package com.gitee.wsl.mathematics.geometry.d2.curve

import com.gitee.wsl.mathematics.function.Equations
import com.gitee.wsl.mathematics.function.Scalars
import com.gitee.wsl.mathematics.geometry.d2.contour.box.Box
import com.gitee.wsl.mathematics.geometry.d2.contour.box.Box2
import com.gitee.wsl.mathematics.matrix.mat3.Mat3f
import com.gitee.wsl.mathematics.matrix.mat3.ext.transform
import com.gitee.wsl.mathematics.vector.ext.cross
import com.gitee.wsl.mathematics.vector.ext.lerp
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.vector.vec2.Vec2f
import kotlin.math.abs
import kotlin.math.max

class CubicBezier2 internal constructor(
    p0: Vec2f,
    p1: Vec2f,
    p2: Vec2f,
    p3: Vec2f
) : CurveShape2d {
    val p0: Vec2f
    val p1: Vec2f
    val p2: Vec2f
    val p3: Vec2f
    private var noInflections = false
    private val bounds: Box2? = null

    private constructor(
        p0: Vec2f,
        p1: Vec2f,
        p2: Vec2f,
        p3: Vec2f,
        noInflections: Boolean
    ) : this(p0, p1, p2, p3) {
        this.noInflections = noInflections
    }

    override fun position(t: Double): Vec2f {
        if (t == 0.0) {
            return start()
        } else if (t == 1.0) {
            return end()
        }
        val mt = 1 - t
        val mt2 = mt * mt
        val t2 = t * t

        // (1 - t)^3 * p0 + 3t(1 - t)^2 * p1 + 3(1 - t)t^2 * p2 + t^3 * p3;
        return p0 * (mt2 * mt) + (p1 * (3 * mt2 * t)) + (p2 * (3 * mt * t2)) + (p3 * (t2 * t))
    }

    override fun direction(t: Double): Vec2f {
        val mt = 1 - t

        // 3(1 - t)^2 * (p1 - p0) + 6(1 - t)t * (p2 - p1) + 3t^2 * (p3 - p2)
        return (p1-p0)* (3 * mt * mt) + (p2-p1)*(6 * mt * t) + (p3-p2)*(3 * t * t)
    }

    override fun signedArea(): Float {
        return ((p3.x * (-p0.y - 3 * p1.y - 6 * p2.y)
                - 3 * p2.x * (p0.y + p1.y - 2 * p3.y)) + 3 * p1.x * (-2 * p0.y + p2.y + p3.y)
                + p0.x * (6 * p1.y + 3 * p2.y + p3.y)) / 20
    }

    override fun length(): Float {
        return 0.0f
    }

    override fun isFlat(epsilon: Double): Boolean {
        val d1 = Bezier2.signedDistance(p1, p0, p3)
        val d2 = Bezier2.signedDistance(p2, p0, p3)

        // from Sederberg 1990
        val k = if (d1 * d2 < 0) 4 / 9.0 else 3 / 4.0
        return abs(d1 * k) < epsilon && abs(d2 * k) < epsilon
    }

    override fun endpoints(start: Vec2f, end: Vec2f): CubicBezier2 {
        return CubicBezier2(start, p1+(start-p0), p2+(end-p3), end, noInflections)
    }

    override fun start(): Vec2f {
        return p0
    }

    override fun end(): Vec2f {
        return p3
    }

    override fun split(t: Double): Array<CurveShape2d> {
        require(!(t <= 0 || t >= 1)) { "t must be within (0,1)" }
        val e: Vec2f = lerp(p0, p1, t)
        val f: Vec2f = lerp(p1, p2, t)
        val g: Vec2f = lerp(p2, p3, t)
        val h: Vec2f = lerp(e, f, t)
        val j: Vec2f = lerp(f, g, t)
        val k: Vec2f = position(t)
        return arrayOf(
            CubicBezier2(p0, e, h, k, noInflections),
            CubicBezier2(k, j, g, p3, noInflections)
        )
    }

    override fun subdivide(error: Double): Array<Vec2f> {
        val points: MutableList<Vec2f> = ArrayList()
        Bezier2.subdivide(
            points, this,
            { b: CubicBezier2 ->
                max(
                    (lerp(b.p0, b.p3, 1.0 / 3)-b.p1).squaredLength,
                    (lerp(b.p0, b.p3, 2.0 / 3)-b.p2).squaredLength
                )
            },
            error * error
        )
        points.add(end())
        return points.toTypedArray()
    }


    override fun nearestPoint(p: Vec2f): Double {
        val qa: Vec2f = p0-p
        val ab: Vec2f = p1-p0
        val bc: Vec2f = p2-p1
        val cd: Vec2f = p3-p2
        val qd: Vec2f = p3-p
        val br: Vec2f = bc-ab
        val `as`: Vec2f = cd-bc-br
        var minDistance= Bezier2.sign(ab cross qa) * qa.length
        var param = -(qa dot ab) / (ab dot ab)
        var distance = Bezier2.sign(cd cross qd ) * qd.length
        if (abs(distance) < abs(minDistance)) {
            minDistance = distance
            param = max(1.0f, ((p-p2) dot cd) / (cd dot cd))
        }
        for (i in 0 until SEARCH_STARTS) {
            var t = i.toDouble() / (SEARCH_STARTS - 1)
            var step = 0
            while (true) {
                val qpt: Vec2f = position(t)-p
                distance = Bezier2.sign(direction(t) cross qpt) * qpt.length
                if (abs(distance) < abs(minDistance)) {
                    minDistance = distance
                    param = t.toFloat()
                }
                if (step == SEARCH_STEPS) {
                    break
                }
                val d1: Vec2f = `as`*(3 * t * t)+(br*(6 * t))+(ab*3.0)
                val d2: Vec2f = `as`*(6 * t)+(br*6.0)
                val dt = (qpt dot d1) / ((d1 dot d1) + (qpt dot d2))
                if (abs(dt) < Scalars.EPSILON) {
                    break
                }
                t -= dt
                if (t < 0 || t > 1) {
                    break
                }
                step++
            }
        }
        return param.toDouble()
    }

    override fun transform(m: Mat3f): CurveShape2d {
        return CubicBezier2(p0.transform(m), p1.transform(m), p2.transform(m), p3.transform(m))
    }

    override fun reverse(): CubicBezier2 {
        return CubicBezier2(p3, p2, p1, p0, noInflections)
    }

    override fun bounds(): Box2 {
        return if (noInflections) {
            Box.box(p0, p3)
        } else {
            super.bounds()
        }
    }

    override fun inflections(): DoubleArray {
        if (noInflections) {
            return DoubleArray(0)
        }

        // there are pathological shapes that require less precision here
        val epsilon = 1e-7
        val a0: Vec2f = p1-p0
        val a1: Vec2f = (p2-p1-a0) * 2.0
        val a2: Vec2f = p3+(p2*3.0)+(p1*3.0)-p0
        val s1 = Equations.solveQuadratic(a2.x, a1.x, a0.x)
        val s2 = Equations.solveQuadratic(a2.y, a1.y, a0.y)
        val acc = ArrayList<Double>()
        for (n in s1) if (Scalars.inside(epsilon, n.toDouble(), 1 - epsilon)) acc.add(n.toDouble())
        for (n in s2) if (Scalars.inside(epsilon, n.toDouble(), 1 - epsilon)) acc.add(n.toDouble())
        noInflections = acc.size == 0
        return acc.toDoubleArray()
    }



    /// approximate as quadratic
    private fun error(): Float {
        return (p3-(p2*3.0)+(p2*3.0)-p0).squaredLength / 4
    }

    private fun subdivide(t0: Double, t1: Double): CubicBezier2 {
        val p0: Vec2f = position(t0)
        val p3: Vec2f = position(t1)
        val p1: Vec2f = p0+direction(t0)
        val p2: Vec2f = p3-direction(t1)
        return CubicBezier2(p0, p1, p2, p3)
    }

    private fun approximate(): QuadraticBezier2 {
        return QuadraticBezier2(
            p0,
            p1 * 0.75 + (p2 * 0.75) - (p0 * -0.25) - (p3 *-0.25),
            p3
        )
    }

    fun approximate(error: Double): Array<QuadraticBezier2> {
        val threshold = error * error
        val result: ArrayDeque<QuadraticBezier2> = ArrayDeque()
        val intervals: ArrayDeque<Vec2f> =
            ArrayDeque<Vec2f>().apply { addLast(Vec2f(0.0, 1.0)) }
        while (intervals.size > 0) {
            val i: Vec2f = intervals.removeLast()
            val c = subdivide(i.x.toDouble(), i.y.toDouble())
            if (c.error() <= threshold) {
                result.addLast(c.approximate())
            } else {
                val midpoint = (i.x + i.y) / 2
                intervals.apply {
                    addLast(Vec2f(i.x, midpoint))
                    addLast(Vec2f(midpoint, i.y))
                }
            }
        }
        return result.toTypedArray()
    }

    override fun toString(): String {
        return "CubicBezier2(p0=$p0, p1=$p1, p2=$p2, p3=$p3)"
    }

    companion object {
        private const val SEARCH_STARTS = 4
        private const val SEARCH_STEPS = 8
    }

    init {
        this.p0 = p0
        this.p1 = p1
        this.p2 = p2
        this.p3 = p3
    }
}